# -*- coding: utf-8 -*-

import threading
from transitions import Machine
from json import load as json_load
import time
import numpy as np

# ==========================================
# * * * * * * * G P I O 初 始 化 * * * * * * * * 
# ==========================================
from platform import system as platform_system
if platform_system()=='Linux':
    import RPi.GPIO as GPIO
    GPIO.setmode(GPIO.BOARD)  
    PIN_START=6
    GPIO.setup(PIN_START,GPIO.IN,pull_up_down=GPIO.PUD_UP)

# In[]
# ==========================================
# * * * * * * * 异 步 读 取 * * * * * * * * 
# ==========================================
class clsFSMThread (threading.Thread):
    def __init__(self,name,delaySeconds=1,dataFeeder=None,actuator=None):
        '''
        一个状态机是一个大脑，应该有一个dataFeeder，用来给状态机喂数据。
        还需要有一个执行器actuator，用来执行操作
        状态机作为中枢管理者，根据喂来的数据，分析结果然后高速actuator该怎么做。
        '''
        threading.Thread.__init__(self)
        self.name = name
        self.exitFlag=False
        self.dataLock=threading.Lock()
        self.dataFeeder=dataFeeder #数据来源
        self.actor=actuator #执行者
        self.delay=delaySeconds
        #-----------------状态机----------------
        self.states = ['stateWaitingEmptyTray','stateEmptyTrayReady','stateLowerDamperReady','stateECUReady', 'stateUpperDamperReady']
        self.transitions=[
            { 'trigger': 'trigEmptyTrayReady', 'source': 'stateWaitingEmptyTray', 'dest': 'stateEmptyTrayReady' },
            { 'trigger': 'trigLowerDamperReady', 'source': 'stateEmptyTrayReady', 'dest': 'stateLowerDamperReady' },
            { 'trigger': 'trigECUReady', 'source': 'stateLowerDamperReady', 'dest': 'stateECUReady' },
            { 'trigger': 'trigUpperDamperReady', 'source': 'stateECUReady', 'dest': 'stateUpperDamperReady' },
            { 'trigger': 'trigWaitingEmptyTray', 'source': 'stateUpperDamperReady', 'dest': 'stateWaitingEmptyTray' },
        ]        
        self.machine = Machine(model=self, states=self.states, initial='stateWaitingEmptyTray',transitions=self.transitions)
        #------------------数据区--------------
        self.numpy_image_RGB=[]
        self.img=[]        
        self.inputString=''
        self.data={}
        self.result={} #各种目标的个数

        #-----------------配置参数-------------
        self.param={}
        self.classes={} # Key: 类别描述，如with_damper...
        #-----------------GPIO初始化-------------
        if platform_system()=='Linux':
            GPIO.add_event_detect(PIN_START,GPIO.FALLING,callback=self.onKeys,bouncetime=100)
        self.startPinPressed=False
        #-----------------Alarm 初始化-------------
        self.alarmQueue=[]

    def onKeys(self,par):
        if(par==PIN_START):
            self.startPinPressed=True
            pass
        pass
    #'''
    def loadConf(self,jsonFn):
        with open(jsonFn) as f:
            self.param = json_load(f)
        print(self.param)
    def loadLabelClass(self,jsonFn):
        with open(jsonFn) as f:
            self.classes = json_load(f)["label_id_group_using"]
            for itm in self.classes:
                self.classes[itm]=int(self.classes[itm])
                self.result[itm+"_count"]=0
    #=================数据获取=================
    def refreshData(self,inData=None):
        '''
        修改这个函数可以从预定义的dataFeeder来获取数据
        返回值：True表示刷新数据成功，False表示没有刷到新数据
        '''
        #res=self.dataFeeder.getInferenceResult()
        res=self.dataFeeder.get_result('objDetector')
        if len(res[0])>1:
            self.data=res[2]
            self.img=res[0] #原始图像
            #if self.data["num_detections"]:print("refresData:",self.data)
            return True
        else:
            return False
    def feedData(self,inData=None):
        '''
        用这个函数可以从任意外部数据源获取数据，用来做测试用。
        '''
        self.data=inData
        pass
    #=================== F S M 核 心 执 行 器 ====================
    def countResult(self):
        if len(self.data)==0:return #避免没有喂到数据时候的错误
        #-----------计算各种目标的个数------------
        for classItem in self.classes:
            condition=self.data["detection_classes"]==self.classes[classItem]
            self.result[classItem+"_count"]=len(self.data["detection_classes"][condition])
        #-----------计算各个box的位置------------
        imgW=len(self.img[0])
        imgH=len(self.img)
        #self.result[]
        for box in self.data["detection_box_centers"]:
            x=int(box[1]*imgW)
            y=int(box[0]*imgH)
            #self.result[]
            #cv2.circle(img1,(x,y),10,(0,255,0),thickness=10)
        #print(self.result)


    def fsmRunOnce(self):
        #self.countResult()
        if self.is_stateWaitingEmptyTray():
            if self.checkEmptyTrayReady():
                self.trigEmptyTrayReady()
            if self.startPinPressed:
                self.startPinPressed=False
                self.alarmQueue.insert(0,101) # "检测到你按下了手柄，但我在等待托盘"
            pass
        elif self.is_stateEmptyTrayReady():
            if self.checkLowerDamperReady():
                self.trigLowerDamperReady()
            elif self.checkECUReady():
                self.alarmQueue.insert(0,202) # "下垫片没有正确放置,检查后再放入ECU"
            if self.startPinPressed:
                self.startPinPressed=False
                self.alarmQueue.insert(0,201) # "检测到你按下了手柄，下垫片还没放好"
            pass
        elif self.is_stateLowerDamperReady():
            if self.checkECUReady():
                self.trigECUReady()
            if self.startPinPressed:
                self.startPinPressed=False
                self.alarmQueue.insert(0,301) # "检测到你按下了手柄，下垫片还没放好"                
            pass
        elif self.is_stateECUReady():
            if self.checkUpperDamperReady():
                self.trigUpperDamperReady()
            if self.startPinPressed:
                self.startPinPressed=False
                self.alarmQueue.insert(0,401) # "检测到你按下了手柄，下垫片还没放好"                
            pass
        elif self.is_stateUpperDamperReady():
            self.trigWaitingEmptyTray()
            pass
        #print(self.state)        
    #===============线程启动和停止==================                
    def run(self):
        """
        FSM线程启动,名称必须为run
        """
        print ("FSM启动：" + self.name)
        self.__thread_FSM__(self.name, self.delay)
        print ("FSM退出：" + self.name)

    def __thread_FSM__(self,threadName, delay):
        while 1:
            if self.exitFlag:
                print('self.exitFlat = ',self.exitFlag)
                break
            else:
                self.dataLock.acquire()
                if(self.refreshData()): #如果刷新数据成功：
                    self.countResult()      #（1）计算各种分类的数量，坐标
                self.fsmRunOnce()           #（2）开始评估状态
                self.dataLock.release()
                time.sleep(delay)
    def exit(self):
        self.exitFlag=True
    #==========================状态检查程序====================
    def checkEmptyTrayReady(self):
        '''
        检查空托盘是否存在
        '''
        if len(self.result)==0:return False
        res=True
        res &= self.result["Lower_fixture_feature_count"]==1
        if(res):print('=================检测到托盘，开始等待下层Damper：')
        return res
    def checkLowerDamperReady(self):
        '''
        检查下面4个垫是否OK
        '''
        if len(self.result)==0:return False
        res=True
        res &= self.result["Lower_with_damper_count"]==4
        res &= self.result["Lower_without_damper_count"]==0
        print('self.result["Lower_with_damper_count"]',self.result["Lower_with_damper_count"])
        if(res):print('=================下面4个垫片OK，等待放入ECU：')
        return res
    def checkECUReady(self):
        '''
        检查是否有ECU
        '''
        if len(self.result)==0:return False
        res=True
        upperDetector=False
        #upperDetector |= self.result["Upper_feature_count"]==1
        upperDetector |= self.result["Upper_yellow_con_count"]==1
        #upperDetector |= self.result["Upper_black_con_count"]==1

        res &= upperDetector
        if(res):print('=================ECU检测到，等待上层4个垫片：')      
        return res            
    def checkUpperDamperReady(self):
        '''
        检查上面4个垫是否OK
        '''
        if len(self.result)==0:return False
        res=True
        res &= self.result["Upper_with_damper_count"]==4
        res &= self.result["Upper_without_damper_count"]==0
        if(res):print('=================上层4个垫片OK，可以推入')
        return res     

    def checkTrayPushedIn(self):
        '''
        检查是否托盘被推进
        '''
        if len(self.result)==0:return False
        res=True
        res &= self.result["Lower_fixture_feature_count"]==0
        res &= self.result["Lower_without_damper_count"]==0
        res &= self.result["Lower_with_damper_count"]==0
        #res &= self.result["Upper_feature_count"]==0
        res &= self.result["Upper_without_damper_count"]==0
        res &= self.result["Upper_with_damper_count"]==0
        res &= self.result["Upper_yellow_con_count"]==0
        #res &= self.result["Upper_black_con_count"]==0

        return res
    #=======================报警函数==================         
    def alarm(self,code):
        if (code ==201):
            #下面的damper没有装全就装ECU了
            print('ALARM: Lower Damper Not Ready but ECU found!')
            pass
        else:
            print('ALARM: alarm code not defined:',code)
        pass    

if __name__=='__main__':
    import os
    PBTXT_PATH=r'../model/saved_model.pbtxt'
    SAVED_MODEL_DIR =os.path.abspath( r'../model/ecu_damper_789/saved_model')
    TEST_IMG_DIR=r'D:\OneDrive - Continental AG\Dataset\ecu_damper\color\20200722-f05'

    from general_utils import clsCamera
    from cores import object_detect
    import mydatafeeder
    cam=clsCamera.clsGeneralCamera(clsCamera.CAMERA.GENERAL_USB)
    camThread=clsCamera.clsCameraThread('camThread',cam)

    obj_detector=object_detect.ObjectDetectionExt(pbtxt_path=PBTXT_PATH,saved_model_dir=SAVED_MODEL_DIR)
    #objThread=object_detect.clsObjectDetectorThread('objDetectThread',obj_detector)

    myFeeder=mydatafeeder.DataFeeder(0.1)
    myFeeder.regist_feeder(
        'cameraThread',
        camThread,
        camThread.getNumpyImage
    )

    myFeeder.regist_processor(
        'objDetector',
        obj_detector,
        obj_detector.putImage_RGB_np,
        'cameraThread',
        obj_detector.startInfer,
        obj_detector.getResultImgAndDicts
    )

    camThread.start()
    myFeeder.start()

    myFSM=clsFSMThread('fsmT',delaySeconds=0.1,dataFeeder=myFeeder)
    myFSM.start()

    while(1):
        res = input('输入命令：\n')
        if(res.strip()=='q'.strip()):
            fsm.exit()
            camThread.exit()
            myFeeder.exit()
            
            break
        else:
            fsm.feedData(res.strip())
